#include "claw_motor.h"
#include "claw.h"
#include "claw_signal.h"
#include "dji2_0.h"

/*刚上电时取箭homing*/
void ClawInitHoming()
{
    for (;;)
    {
        dji_Homing(ClawCAN, ClawBoardID, ClawMoveID, -300, 3000);
        uint32_t evt = osEventFlagsWait(EventTakeArrowHandle, MOVE_FINISH_HOMING, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (MOVE_FINISH_HOMING))) {
            break;
        }
    }   
    uprintf("Claw Homing Init Done!\r\n");
}

/*三个抓手状态控制，0为开，1为关*/
void ClawCtr(uint8_t state1, uint8_t state2, uint8_t state3)
{
    can_msg msg;
    uint32_t ClawID = 220;

    if (state1 == 0)
        msg.ui16[0] = 300;
    else 
        msg.ui16[0] = 486;
    if (state2 == 0)
        msg.ui16[1] = 300;
    else 
        msg.ui16[1] = 469;
    if (state3 == 0)
        msg.ui16[2] = 300;
    else 
        msg.ui16[2] = 473;

    OSLIB_CAN_SendMessage(ClawCAN, CAN_ID_STD, ClawID, &msg);
}

/*取箭三个大疆电机位置环初始化*/
void ClawMotorInit()
{
    dji_PosCfg(ClawCAN, ClawBoardID, ClawRiseID, ClawRiseSpeed);
    osDelay(20);
    dji_PosCfg(ClawCAN, ClawBoardID, ClawRollID, ClawRollSpeed);
    osDelay(20);
    dji_PosCfg(ClawCAN, ClawBoardID, ClawMoveID, ClawMoveSpeed);
    osDelay(20);
    uprintf("Claw Motor Init Done!");
}

/*取箭三个大疆电机位置归零*/
void ClawMotorReset()
{
    dji_Homing(ClawCAN, ClawBoardID, ClawRiseID, -50, 3000);
    osDelay(20);
    dji_Homing(ClawCAN, ClawBoardID, ClawRollID, 30, 5000);
    osDelay(20);
    dji_Homing(ClawCAN, ClawBoardID, ClawMoveID, -250, 3000);
    osDelay(20);
    uprintf("claw all motor reset!\r\n");
}

/*取箭抓手移动到第一次取箭位置*/
void claw_move_first_pos()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawMoveID, ClawFirstPos);
        evt = osEventFlagsWait(EventTakeArrowHandle, MOVE_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (MOVE_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw move to first position!\r\n");
}

/*取箭抓手移动到第二次取箭位置*/
void claw_move_second_pos()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawMoveID, ClawSecondPos);
        evt = osEventFlagsWait(EventTakeArrowHandle, MOVE_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (MOVE_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw move to second position!\r\n");
}

/*取箭抓手刚上升时翻转小角度*/
void claw_roll_little()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawRollID, ClawRollLittle);
        evt = osEventFlagsWait(EventTakeArrowHandle, ROLL_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (ROLL_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw roll little ok!\r\n");
}

/*取箭抓手上升到最高点*/
void claw_rise()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawRiseID, ClawRiseHeight); //上升
        evt = osEventFlagsWait(EventTakeArrowHandle, RISE_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (RISE_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw rise to highest ok!\r\n");
}

/*取箭抓手翻转到限位块*/
void claw_roll_homing()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_Homing(ClawCAN, ClawBoardID, ClawRollID, -ClawRollSpeed, 10000); //上翻至限位
        evt = osEventFlagsWait(EventTakeArrowHandle, ROLL_FINSIH_HOMING, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (ROLL_FINSIH_HOMING))) {
            break;
        }
    }   
    uprintf("Claw roll homing ok!\r\n");
}

/*取箭移动到放第一支箭位置*/
void claw_move_first_arrow()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawMoveID, ClawMovePos1);  //移动第一支箭位置
        evt = osEventFlagsWait(EventTakeArrowHandle, MOVE_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (MOVE_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw move to first arrow!\r\n");
}

/*取箭移动到放第二支箭位置*/
void claw_move_second_arrow()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawMoveID, ClawMovePos2);  //移动第二支箭位置
        evt = osEventFlagsWait(EventTakeArrowHandle, MOVE_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (MOVE_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw move to second arrow!\r\n");
}

/*取箭移动到第三支箭位置*/
void claw_move_third_arrow()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawMoveID, ClawMovePos3);  //移动第三支箭位置
        evt = osEventFlagsWait(EventTakeArrowHandle, MOVE_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (MOVE_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw move to third arrow!\r\n");
}

/*取箭松手放箭时先下降一小段*/
void claw_drop_little()
{
    uint32_t evt = 0;
    for (;;)
    {
        dji_PosCtrl(ClawCAN, ClawBoardID, ClawRiseID, ClawLoosenHeight);  //放箭时先下降一小段
        evt = osEventFlagsWait(EventTakeArrowHandle, RISE_GET_POSITION, osFlagsWaitAny, 500);
        if (evt == (uint32_t)osErrorTimeout) {
            continue;
        }
        else if (evt<osStatusReserved && (evt & (RISE_GET_POSITION))) {
            break;
        }
    }   
    uprintf("Claw drop to loosen height!\r\n");
}

/*一轮放箭结束抓手下翻下降左移*/
void claw_reset()
{
    uint32_t reset_flag = 0;
    uint32_t evt = 0;
    dji_Homing(ClawCAN, ClawBoardID, ClawRollID, ClawRollSpeed, 6000); //下翻至限位
    dji_PosCtrl(ClawCAN, ClawBoardID, ClawRiseID, 1000);               //抓手下降
    dji_Homing(ClawCAN, ClawBoardID, ClawMoveID, -300, 3000);          //左移到原点
    for (;;)
    {
        evt = osEventFlagsWait(EventTakeArrowHandle, RISE_GET_POSITION|ROLL_FINSIH_HOMING|MOVE_FINISH_HOMING, osFlagsWaitAll, 500);
        reset_flag = osEventFlagsGet(EventTakeArrowHandle);
        if (evt == (uint32_t)osErrorTimeout) {
            if (!(reset_flag & RISE_GET_POSITION)) {
                dji_PosCtrl(ClawCAN, ClawBoardID, ClawRiseID, 1000);
            }
            if (!(reset_flag & ROLL_FINSIH_HOMING)) {
                dji_Homing(ClawCAN, ClawBoardID, ClawRollID, ClawRollSpeed, 6000); //下翻至限位
            }
            if (!(reset_flag & MOVE_FINISH_HOMING)) {
                dji_Homing(ClawCAN, ClawBoardID, ClawMoveID, -300, 3000);          //左移到原点
            }
            continue;
        }
        else if (evt<osStatusReserved && (evt & (RISE_GET_POSITION|ROLL_FINSIH_HOMING|MOVE_FINISH_HOMING))) {
            break;
        }
    }
    uprintf("Claw reset ok!\r\n");
}



